#include "sv2_mot.h"
#include <cmath>
#include <opencv2/calib3d.hpp>
#include <opencv2/dnn.hpp>
#include <fstream>
#include <unistd.h>
// #include <ros/ros.h>
// #include <std_msgs/Float32.h>
// #include "sv_world.h"

// void trackIDCallback(const std_msgs::Float32 &msg)
// {
//   std::cout<<"selected_id: "<< msg->data<<endl;
// }
namespace sv2 {

using namespace cv;
using namespace cv::dnn;
using namespace Eigen;



void MOTCppNode::run()
{
  while (this->is_running())
  {
    cv::Mat image;
    // // 主线程阻塞，等待队里中出现数据
    std::unique_lock<std::mutex> lock(this->_image_queue_mtx);
    this->_image_cv.wait(lock, [this] { return !this->_image_queue.empty(); });
    
    // nlohmann::json json_image = this->_image_queue.front();
    // this->_image_queue.pop();

    ImgMsg imgmsg = this->_image_queue.front();
    imgmsg.img.copyTo(image);
    nlohmann::json json_image=imgmsg.msg;
    this->_image_queue.pop();
    nlohmann::json res_msg = this->track(image, json_image);
    json_image["spirecv_msgs::2DTargets"] = res_msg;
    _vis_pub.publish(json_image);

    // if (this->_has_roi)
    //   {
    //     continue;
    //   }
    // if (this->_pause)
    // {
    //   continue;
    // }
    // else
    //   {
    //     // cv::Mat image = sms::sms2cvimg(json_image);
    //     nlohmann::json res_msg = this->track(image, json_image);
    //     json_image["spirecv_msgs::2DTargets"] = res_msg;
    //     _vis_pub.publish(json_image);
    //   }

    // // cv::imshow("image", image);
    // // cv::waitKey(5);

  }
}


nlohmann::json MOTCppNode::track(cv::Mat img_, nlohmann::json json_image)
{
  nlohmann::json targets_msg = sms::def_msg("spirecv_msgs::2DTargets");
  targets_msg = json_image["spirecv_msgs::2DTargets"];
  nlohmann::json tracked_targets = sms::def_msg("spirecv_msgs::2DTargets");

  int frame_id = (int)(targets_msg["img_id"]);
  logger.info("frame id: "+ std::to_string(frame_id));

  for (const auto& target : targets_msg["targets"])
  {
    int cat_id = (int)(target["category_id"]);
    // if (cat_id == this->_tracked_dictionary_id)
    if(std::find(this->_tracked_dictionary_id.begin(),this->_tracked_dictionary_id.end(), -1) != this->_tracked_dictionary_id.end())
    {
       logger.info("tracked all cls obj.");
       tracked_targets["targets"].push_back(target);
       continue;
    }

    if(std::find(this->_tracked_dictionary_id.begin(),this->_tracked_dictionary_id.end(), cat_id) != this->_tracked_dictionary_id.end())
    {
       logger.info("obj id: "+ std::to_string(cat_id));
       tracked_targets["targets"].push_back(target);
    }
  }

  nlohmann::json ids_msg = sms::def_msg("spirecv_msgs::2DTargets");
  logger.info("start tracking." );
  ids_msg = this->_bytetrack_impl->update(tracked_targets);
  //std::cout<<"size: "<<(int)(tracked_targets["targets"].size())<<std::endl;
  nlohmann::json msg = sms::def_msg("spirecv_msgs::2DTargets");
  if(ids_msg["targets"].size()>0)
  {
  //std::cout<<(int)(ids_msg["targets"][0]["tracked_id"])<<std::endl;
  msg = this->_publish_to_sms(tracked_targets, ids_msg, img_.cols, img_.rows);
  _res_pub.publish(msg);
  }
  return msg;  
}


// double MOTCppNode::_calc_yaw(double vec_x_, double vec_y_)
// {
//   double yaw_a;
//   if (vec_x_ == 0. && vec_y_ > 0.)
//   {
//     yaw_a = 180;
//   }
//   else if (vec_x_ == 0. && vec_y_ < 0.)
//   {
//     yaw_a = 0;
//   }
//   else if (vec_x_ > 0. && vec_y_ == 0.)
//   {
//     yaw_a = 90;
//   }
//   else if (vec_x_ > 0. && vec_y_ > 0.)
//   {
//     yaw_a = 180 - atan(vec_x_ / vec_y_) * SV2_RAD2DEG;
//   }
//   else if (vec_x_ > 0. && vec_y_ < 0.)
//   {
//     yaw_a = atan(vec_x_ / -vec_y_) * SV2_RAD2DEG;
//   }
//   else if (vec_x_ < 0. && vec_y_ == 0.)
//   {
//     yaw_a = -90;
//   }
//   else if (vec_x_ < 0. && vec_y_ > 0.)
//   {
//     yaw_a = atan(-vec_x_ / vec_y_) * SV2_RAD2DEG - 180;
//   }
//   else if (vec_x_ < 0. && vec_y_ < 0.)
//   {
//     yaw_a = -atan(-vec_x_ / -vec_y_) * SV2_RAD2DEG;
//   }
//   return yaw_a;
// }
double MOTCppNode::_calculate_pixels(double center, int image_shape)
{
  double pixel = (double)(center / image_shape);
  double pixel_angle = (pixel - 0.5) / 0.5;
  return pixel_angle;
}


nlohmann::json MOTCppNode::_publish_to_sms(
  nlohmann::json tracked_targets,
  nlohmann::json ids_msg,
  int im_width,
  int im_height
)
{
  nlohmann::json msg = sms::def_msg("spirecv_msgs::2DTargets");
  msg["width"] = im_width;
  msg["height"] = im_height;
  logger.info("width: "+ std::to_string(im_width));
  // msg["fov_x"] = this->fov_x;
  // msg["fov_y"] = this->fov_y;
  if (this->_t0 > 0)
  {
    msg["fps"] = 1.0 / (sms::get_time_sec() - this->_t0 + 1e-6);
  }
  else
  {
    msg["fps"] = 0;
  }
  this->_t0 = sms::get_time_sec();
  // logger.info("tracked targets: "+ std::to_string(tracked_targets["targets"].size()));
  // logger.info("ids targets: "+ std::to_string(ids_msg["targets"].size()));
  msg["targets"] = nlohmann::json::array();
  for(int i=0;i<ids_msg["targets"].size();i++)
  {
    nlohmann::json target;
    target = ids_msg["targets"][i];
    // for(int j=0;j<ids_msg["targets"][i]["bbox"].size();j++)
    // {
    //   std::cout<<(int)(ids_msg["targets"][i]["bbox"][j])<<std::endl;
    // }
    // double left = (double)(ids_msg["targets"][i]["bbox"][0]);
    // double top = (double)(ids_msg["targets"][i]["bbox"][1]);
    // double right = left + (double)(ids_msg["targets"][i]["bbox"][2]);
    // double bottom = top + (double)(ids_msg["targets"][i]["bbox"][3]);
    // target["bbox"].push_back({left, top, right, bottom});
    target["score"] = tracked_targets["targets"][i]["score"];
     int cat_id = (int)(tracked_targets["targets"][i]["category_id"]);
    target["category_name"] = tracked_targets["targets"][i]["category_name"];

    double left = (double)(target["bbox"][0]);
    double top = (double)(target["bbox"][1]);
    double right = left + (double)(target["bbox"][2]);
    double bottom = top + (double)(target["bbox"][3]);
    double center_x = (left+right)/2;
    double center_y = (top+bottom)/2;

    double pixel_x = this->_calculate_pixels(center_x, im_width);
    double pixel_y = this->_calculate_pixels(center_y, im_height);
    target["pixel_x"]=pixel_x;
    target["pixel_y"]=pixel_y;

    double los_ax = 4.8 * pixel_x;
    double los_ay = 2.7 * pixel_y;
    //double los_ax = atan((center_x - im_width / 2.) / this->camera_matrix.at<double>(0, 0)) * SV2_RAD2DEG* 2 / 8.; //* SV2_RAD2DEG now show in 'rad'. 
    //double los_ay = atan((center_y - im_height / 2.) / this->camera_matrix.at<double>(1, 1)) * SV2_RAD2DEG* 3 / 10.;//* SV2_RAD2DEG
    // logger.info("los_ax: "+ std::to_string(los_ax)+", los_ay: "+std::to_string(los_ay));
    target["los_ax"]=los_ax;
    target["los_ay"]=los_ay;

    if(ids_msg["targets"][i].contains("tracked_id"))
    {
    // target["tracked_id"] = ids_msg["targets"][i]["tracked_id"];
    logger.info("tracked id: "+ std::to_string((int)(target["tracked_id"]))+". los_ax: "+ std::to_string(los_ax)+", los_ay: "+std::to_string(los_ay));
    if ((int)(target["tracked_id"]) == this->_selected_id)
    { 
      logger.info("selected id.");
      // double left = (double)(tracked_targets["targets"][i]["bbox"][0]);
      // double top = (double)(tracked_targets["targets"][i]["bbox"][1]);
      // double right = left + (double)(tracked_targets["targets"][i]["bbox"][2]);
      // double bottom = top + (double)(tracked_targets["targets"][i]["bbox"][3]);
      double left = (double)(ids_msg["targets"][i]["bbox"][0]);
      double top = (double)(ids_msg["targets"][i]["bbox"][1]);
      double right = left + (double)(ids_msg["targets"][i]["bbox"][2]);
      double bottom = top + (double)(ids_msg["targets"][i]["bbox"][3]);
      std::vector<int> roi;
      roi.push_back((int)round(left));
      roi.push_back((int)round(top));
      roi.push_back((int)round(right));
      roi.push_back((int)round(bottom));
      msg["rois"].push_back({left, top, right, bottom});
      }

    }


  // for (int i=0; i<tracked_targets["targets"].size(); i++)
  // {
  //   nlohmann::json target;
  //   target = tracked_targets["targets"][i];
  //   target["score"] = tracked_targets["targets"][i]["score"];
  //   int cat_id = (int)(tracked_targets["targets"][i]["category_id"]);
  //   target["category_name"] = tracked_targets["targets"][i]["category_name"];//"d" + std::to_string(_tracked_dictionary_id) + "-" + std::to_string(cat_id);
  //   if(ids_msg["targets"][i].contains("tracked_id"))
  //   {
  //   target["tracked_id"] = ids_msg["targets"][i]["tracked_id"];
  //   logger.info("tracked id: "+ std::to_string((int)(target["tracked_id"])));
  //   if ((int)(target["tracked_id"]) == this->_selected_id)
  //   { 
  //     logger.info("selected id.");
  //     double left = (double)(tracked_targets["targets"][i]["bbox"][0]);
  //     double top = (double)(tracked_targets["targets"][i]["bbox"][1]);
  //     double right = left + (double)(tracked_targets["targets"][i]["bbox"][2]);
  //     double bottom = top + (double)(tracked_targets["targets"][i]["bbox"][3]);
  //     std::vector<int> roi;
  //     roi.push_back((int)round(left));
  //     roi.push_back((int)round(top));
  //     roi.push_back((int)round(right));
  //     roi.push_back((int)round(bottom));
  //     msg["rois"].push_back({left, top, right, bottom});
  //     }
  //   }



    // target["category_id"] = tracked_targets["targets"][i]["category_id"];
    // target["instance_id"] = tracked_targets["targets"][i]["instances_id"];
    // double left = (double)(tracked_targets["targets"][i]["bbox"][0]);
    // double top = (double)(tracked_targets["targets"][i]["bbox"][1]);
    // double right = left + (double)(tracked_targets["targets"][i]["bbox"][2]);
    // double bottom = top + (double)(tracked_targets["targets"][i]["bbox"][3]);
    // // double left = std::min(std::min(corner_[0].x, corner_[1].x), std::min(corner_[2].x, corner_[3].x));
    // // double right = std::max(std::max(corner_[0].x, corner_[1].x), std::max(corner_[2].x, corner_[3].x));
    // // double top = std::min(std::min(corner_[0].y, corner_[1].y), std::min(corner_[2].y, corner_[3].y));
    // // double bottom = std::max(std::max(corner_[0].y, corner_[1].y), std::max(corner_[2].y, corner_[3].y));
    // // target["bbox"] = {left, top, right - left + 1, bottom - top + 1};
    // target["bbox"] = tracked_targets["targets"][i]["bbox"];


    // double x_mid = (corner_[0].x + corner_[1].x) / 2.;
    // double y_mid = (corner_[0].y + corner_[1].y) / 2.;
    // double x_vec = x_mid - (left + right) / 2.;
    // double y_vec = y_mid - (top + bottom) / 2.;
    // target["yaw_a"] = this->_calc_yaw(x_vec, y_vec);

    // double cx = (double)(right + left) / 2 / im_width;
    // double cy = (double)(bottom + top) / 2 / im_height;
    // double los_ax = atan((cx * im_width - im_width / 2.) / this->camera_matrix.at<double>(0, 0)) * SV2_RAD2DEG;
    // double los_ay = atan((cy * im_height - im_height / 2.) / this->camera_matrix.at<double>(1, 1)) * SV2_RAD2DEG;
    // target["los"] = {los_ax, los_ay};
    // target["position"] = {tvecs_[i][0], tvecs_[i][1], tvecs_[i][2]};
    // target["rvec"] = {rvecs_[i][0], rvecs_[i][1], rvecs_[i][2]};

    msg["targets"].push_back(target);
  }
  return msg;
}


}
